#include <Wire.h>
#include <HardwareSerial.h>
#include <Arduino.h>
#include "GPSSerial.h"
#include "MMGi2c.h"
#include "SoftSerial.h"
#include "multiplexer.h"
#include "packets.h"

#define SENSOR_COUNT 2

char packetData[4+SENSOR_COUNT*2];  	// array to store the content of a data packet, time+sensors+gps
unsigned long* timeStamp=(unsigned long*)packetData;		// pointer to time stamp in data packet content
unsigned short* sensorData=(unsigned short*)(packetData+4); // pointer to beginning of sensor section of data content
Packet dataPacket;					// re-usable Packet structure for storing the data packet
Packet rxPacket; 					// Packet structure for receiving a packet
unsigned long t0;
									

//Array containing the device configuration section
//each sensor is represented by 2 bytes, first is device type ID
//second is data format type ID
char DeviceConfig[]={
  SDID_ELYCRA,DFID_UINT16,
  SDID_FLEX,DFID_UINT16
};

void setup(){			
  Serial.begin(115200);		// Serial used for Bluetooth 
  initialiseMMG();
  
  pinMode(13,OUTPUT);
  digitalWrite(13,HIGH);

  dataPacket.StartPacket=PACKET_START;	// Set Packet start byte
  dataPacket.EndPacket=PACKET_END;		// Set Packet end byte
  dataPacket.MessageType=DATA_MESSAGE;  // set Packet type to Data Message
  dataPacket.DataContent=packetData;	// set data content pointer to buffer
  
  connect();
}

void connect(){ // Waits for a connection request from the PC and then connects
  Packet txPacket;										// Packet structure for sending a packet
  
  do{
    do{
      while(!readPacket(&rxPacket));					 // wait until a packet is received
	}
    while(rxPacket.MessageType!=INITIALISATION_REQUEST); // make sure it's an initialisation request message, otherwise wait for another packet
    txPacket.StartPacket=PACKET_START;					 // Initialise the Trasmit packet
    txPacket.EndPacket=PACKET_END;
    txPacket.MessageType=DEVICE_CONFIGURATION_STATUS;    // Set packet type to device configuration packet
    txPacket.DataLength=sizeof(DeviceConfig);
    txPacket.DataContent=DeviceConfig;					 // Set data content of packet to device configuration
    updateChecksum(&txPacket);
    sendPacket(&txPacket);								 // Send device configuration packet to the PC
    while(!readPacket(&rxPacket));						 // Wait for a packet to  be received
  }
  while(rxPacket.MessageType!=MESSAGE_ACK);				 // Make sure it's an acknowledgement packet
  t0=millis();											 // Record the start time
}

unsigned long lastLoopTime=0;

void loop(){
  while(millis()<lastLoopTime+50);						// Wait until 50 ms have passed since the last measurement (limits to 20 samples/s) 
  char i;
  *timeStamp=millis()-t0;								// set time stamp in ms to time since switch on.
  sensorData[0]=analogRead(A0);
  sensorData[1]=analogRead(A1);										// check for new GPS data

  dataPacket.DataLength=4+2*SENSOR_COUNT; // re-calculate data length for data content
  updateChecksum(&dataPacket);
  sendPacket(&dataPacket);								// send data packet to PC
  lastLoopTime=millis();								// store time that data sent to limit rate to 20 samples/s

  if(readPacket(&rxPacket)){
    if(rxPacket.MessageType==DISCONNECT_REQUEST){		// if disconnect request received wait for a new connect request
	  connect();
	}
  }
}
